#include "rgbd_kinect2.h"

#include <libfreenect2/packet_pipeline.h>
#include <libfreenect2/logger.h>
#include <pcl/io/pcd_io.h>
#define _USE_MATH_DEFINES
#include <math.h>
#include <limits>
#define Frame_W	512
#define Frame_H	424

RGBD_Kinect2::RGBD_Kinect2(){
    // create a console logger with debug level (default is console logger with info level)
}
void RGBD_Kinect2::init(){
    libfreenect2::setGlobalLogger(libfreenect2::createConsoleLogger(libfreenect2::Logger::Error));

    std::string serial = "";

    if(this->freenect2.enumerateDevices() == 0)
    {
        std::cout << "no device connected!" << std::endl;
        this->Failed = true;
        return;
    }

    if(this->pipeline == nullptr)
        this->pipeline = new libfreenect2::OpenGLPacketPipeline();

    if (serial == "")
        serial = this->freenect2.getDefaultDeviceSerialNumber();

    if(this->pipeline)
        this->dev = this->freenect2.openDevice(serial, this->pipeline);
    else
        this->dev = this->freenect2.openDevice(serial);

    if(this->dev == 0)
    {
        std::cout << "failure opening device!" << std::endl;
        this->Failed = true;
        return;
    }

    this->listener = new libfreenect2::SyncMultiFrameListener(
                libfreenect2::Frame::Color |
                libfreenect2::Frame::Depth
                );
    this->frames = new libfreenect2::FrameMap();

    this->dev->setColorFrameListener(this->listener);
    this->dev->setIrAndDepthFrameListener(this->listener);

    if (!this->dev->start()){
        std::cout << "failure starting device!" << std::endl;
        this->Failed = true;
        return;
    }

    std::cout << "device serial: " << this->dev->getSerialNumber() << std::endl;
    std::cout << "device firmware: " << this->dev->getFirmwareVersion() << std::endl;

    this->registration = new libfreenect2::Registration(
                this->dev->getIrCameraParams(),
                this->dev->getColorCameraParams()
                );
    this->undistorted = new libfreenect2::Frame(Frame_W, Frame_H, 4);
    this->registered = new libfreenect2::Frame(Frame_W, Frame_H, 4);

    this->depth_param = this->dev->getIrCameraParams();
}


RGBD_Kinect2::~RGBD_Kinect2()
{
    if(this->dev){
        this->dev->stop();
        this->dev->close();
        delete this->dev;
        delete this->listener;
        delete this->frames;
        delete this->registered;
        delete this->undistorted;
        delete this->registration;
    }
}

int RGBD_Kinect2::fetchXYZ(pcl::PointCloud<pcl::PointXYZ> *XYZ,
                           float limit_near, float limit_far)
{
    if(!this->fetchAndLockFrame()) return -1;
    return this->convertToXYZ(this->undistorted, XYZ, limit_near, limit_far);
}

int RGBD_Kinect2::fetchXYZRGB(pcl::PointCloud<pcl::PointXYZRGB> *XYZRGB,
                              float limit_near, float limit_far)
{
    if(!this->fetchAndLockFrame()) return -1;
    return this->convertToXYZRGB(this->undistorted, this->registered, XYZRGB, limit_near, limit_far);
}

int RGBD_Kinect2::fetchColorImage(cv::Mat &image){
    if(!this->fetchAndLockFrame()) return -1;
    image = this->color_image.clone();
    return 1;
}

int RGBD_Kinect2::fetchHdColorImage(cv::Mat &image){
    if(!this->fetchAndLockFrame()) return -1;
    image = this->color_hd_image.clone();
    return 1;
}

int RGBD_Kinect2::fetchDepthImage(cv::Mat &image){
    if(!this->fetchAndLockFrame()) return -1;
    image = this->depth_image.clone();
    return 1;
}


bool RGBD_Kinect2::fetchAndLockFrame()
{
    if(this->Failed)
        return false;
    if(!this->listener->waitForNewFrame(*this->frames, 10*1000)) // 10 sconds
    {
        std::cout << "timeout!" << std::endl;
        return false;
    }
    libfreenect2::Frame * rgb = (*this->frames)[libfreenect2::Frame::Color];
    libfreenect2::Frame * depth = (*this->frames)[libfreenect2::Frame::Depth];

    this->registration->apply(
               rgb, depth,
            this->undistorted, this->registered
            );
    this->color_image = cv::Mat(this->registered->height, this->registered->width, CV_8UC4, this->registered->data).clone();
    this->depth_image = cv::Mat(this->undistorted->height, this->undistorted->width, CV_32FC1, this->undistorted->data).clone();
    this->color_hd_image = cv::Mat(rgb->height, rgb->width, CV_8UC4, rgb->data).clone();

    cv::flip( this->color_image,  this->color_image, 1);
    cv::flip( this->depth_image,  this->depth_image, 1);
    cv::flip( this->color_hd_image,  this->color_hd_image, 1);

    this->listener->release(*this->frames);
    this->framecount++;
    return true;
}

int RGBD_Kinect2::convertToXYZ(const libfreenect2::Frame *undistorted,
                               pcl::PointCloud<pcl::PointXYZ> *XYZ_vect,
                               float limit_near, float limit_far)
{
    const float cx(this->depth_param.cx), cy(this->depth_param.cy);
    const float fx(1.0 / this->depth_param.fx), fy(1.0 / this->depth_param.fy);
    float* undistorted_data = (float *)(undistorted->data);

    XYZ_vect->clear();
    this->bbox.clear();

    int current_idx = 0;

    for(uint32_t i = 0 ; i < undistorted->width * undistorted->height ; i++){
        float depth_val = undistorted_data[i]/1000.0f; //scaling factor, so that value of 1 is one meter.
        int c = (i & 511);
        int r = (i >> 9);
        pcl::PointCloud<pcl::PointXYZ>::PointType p;
        p.x = -(c + 0.5 - cx) * fx * depth_val;
        p.y = (r + 0.5 - cy) * fy * depth_val;
        p.z = depth_val;
        this->bbox.setP(p.x, p.y, p.z);
        XYZ_vect->push_back(p);
        current_idx++;
    }
    XYZ_vect->height = undistorted->height;
    XYZ_vect->width = undistorted->width;
    return undistorted->width * undistorted->height - current_idx;
}

int RGBD_Kinect2::convertToXYZRGB(const libfreenect2::Frame *undistorted,
                                  const libfreenect2::Frame *registered,
                                  pcl::PointCloud<pcl::PointXYZRGB> *XYZRGB_vect
                                  , float limit_near, float limit_far)
{
    const float cx(this->depth_param.cx), cy(this->depth_param.cy);
    const float fx(1.0 / this->depth_param.fx), fy(1.0 / this->depth_param.fy);
    float* undistorted_data = (float*)undistorted->data;
    uint32_t* registered_data = (uint32_t*)registered->data;

    XYZRGB_vect->clear();
    this->bbox.clear();

    int current_idx = 0;

    for(uint32_t i = 0 ; i < undistorted->width * undistorted->height ; i++){
        float depth_val = undistorted_data[i]/1000.0f; //scaling factor, so that value of 1 is one meter.
        float c = (i & 511);
        float r = (i >> 9);
        pcl::PointCloud<pcl::PointXYZRGB>::PointType p;
        p.rgba = registered_data[i];
        p.x = -(c + 0.5 - cx) * fx * depth_val;
        p.y = (r + 0.5 - cy) * fy * depth_val;
        p.z = depth_val;
        this->bbox.setP(p.x, p.y, p.z);
        XYZRGB_vect->push_back(p);
        current_idx++;
    }
    XYZRGB_vect->height = undistorted->height;
    XYZRGB_vect->width = undistorted->width;

    return undistorted->width * undistorted->height - current_idx;
}

void RGBD_Kinect2::BoundingBox::clear()
{
    this->x_max = this->y_max = this->z_max = std::numeric_limits<float>::min();
    this->x_min = this->y_min = this->z_min = std::numeric_limits<float>::max();
}

void RGBD_Kinect2::BoundingBox::setP(float x, float y, float z)
{
    if(x > this->x_max) x_max = x;
    if(x < this->x_min) x_min = x;
    if(y > this->y_max) y_max = y;
    if(y < this->y_min) y_min = y;
    if(z > this->z_max) z_max = z;
    if(z < this->z_min) z_min = z;
}
